/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-10     Hao       the first version
 */
#include "can_upper_task.h"

#include "motor_task.h"
#include <hardware/can_operate.h>
#include <hardware/gpio_operate.h>

typedef union _can_id
{
    uint32_t id;
    struct
    {
        uint32_t dst :4;
        uint32_t src :4;
        uint32_t type :3;
        uint32_t :21;
    } bits;
} can_id_t;

static void pack_id(uint32_t *out_id, uint8_t type, uint8_t src_id, uint8_t dst_id)
{
    can_id_t temp_id;
    temp_id.id = 0;

    temp_id.bits.type = type;
    temp_id.bits.src = src_id;
    temp_id.bits.dst = dst_id;

    *out_id = temp_id.id;
}

static void unpack_id(uint32_t in_id, uint8_t *type, uint8_t *src_id, uint8_t *dst_id)
{
    can_id_t temp_id;
    temp_id.id = in_id;

    *type = temp_id.bits.type;
    *src_id = temp_id.bits.src;
    *dst_id = temp_id.bits.dst;
}

static rt_size_t send_to_upper(can_ptr port_ptr, uint8_t type, uint8_t src_id, uint8_t dst_id, rt_uint8_t *buf, rt_uint8_t len)
{
    uint32_t temp_id;

    pack_id(&temp_id, type, src_id, dst_id);

    return can_send(port_ptr, temp_id, buf, len);
}

static rt_size_t recv_from_upper(can_ptr port_ptr, uint8_t *type, uint8_t *src_id, uint8_t *dst_id, rt_uint8_t *buf, rt_uint8_t *len)
{
    rt_size_t size;
    rt_uint32_t temp_id;

    size = can_recv(port_ptr, &temp_id, buf, len);

    unpack_id(temp_id, type, src_id, dst_id);

    return size;
}

// 上报运动实际值、接收目标值都为浮点数，需要和uint8_t相互转换
typedef enum
{
    BIG = 0,
    LITTLE
} data_type;

typedef union float_uint8_
{
    float data_float;
    uint8_t data_8[4];
} float_uint8;

// 把一个float类型的数据，拆解成4个uint8_t类型的善举
void float_to_uint8(float in, uint8_t *out_buff, data_type mode)
{
    float_uint8 temp;
    temp.data_float = in;

    if (mode == LITTLE)
    {
        rt_memcpy(out_buff, temp.data_8, 4);
    }
    else if (mode == BIG)
    {
        out_buff[0] = temp.data_8[3];
        out_buff[1] = temp.data_8[2];
        out_buff[2] = temp.data_8[1];
        out_buff[3] = temp.data_8[0];
    }
}

// 把4个uint8_t类型的数据，组合成一个float类型的数据
void uint8_to_float(uint8_t *in_buff, float *out, data_type mode)
{
    float_uint8 temp;

    if (mode == LITTLE)
    {
        rt_memcpy(temp.data_8, in_buff, 4);
    }
    else if (mode == BIG)
    {
        temp.data_8[0] = in_buff[3];
        temp.data_8[1] = in_buff[2];
        temp.data_8[2] = in_buff[1];
        temp.data_8[3] = in_buff[0];
    }
    *out = temp.data_float;
}

void can_upper_task(void *para)
{
    rt_err_t ret;
    uint8_t src_id, dst_id, type, len;
    uint8_t rx_buf[8] = { 0 };
    uint8_t send_buf[8] = { 0 };

    // 计算本设备CAN地址，1、2、3、4
    uint8_t can_addr = gpio_read(&can_addr1_pin) * 1 + gpio_read(&can_addr2_pin) * 2 + 1;

    // 初始化can设备
    can_operate_init(&can_dev);

    while (1)
    {
        // 等待接收消息，等待500ms
        ret = rt_sem_take(can_dev.rx_sem, 500);
        if (ret == RT_EOK)
        {
            // 接收一条消息进行处理
            recv_from_upper(&can_dev, &type, &src_id, &dst_id, rx_buf, &len);

            // 判断是否是发给本板卡的消息（实际上设置硬件过滤的话 已经过滤过了）
            if ((dst_id == can_addr) && (src_id == UPPER_ADDR))
            {
                // 执行相应的功能
                if (type == SET_MODE)
                {
                    motor_mode = rx_buf[0]; // 模式参考MotionControlType定义,后续会改成应用模式定义，不是电机应用
                }
                else if (type == SET_TARGET)
                {
                    uint8_to_float(rx_buf, &motor_target, LITTLE);
                }
            }
            rt_memset(rx_buf, 0, len);
        }
        else    // 发送当前运行实际值
        {
            float_to_uint8(motor_angle, send_buf, LITTLE);
            send_to_upper(&can_dev, REPORT_DATA, can_addr, UPPER_ADDR, send_buf, 8);
        }
    }
}
